/*
* Copyright (c) 2006-2007 Erin Catto http:
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked, and must not be
* misrepresented the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

goog.provide('box2d.ContactSolver');
goog.require('box2d.ContactConstraint');
goog.require('box2d.Settings');

/**
 @constructor
 */
box2d.ContactSolver = function(contacts, contactCount) {
  // initialize instance variables for references
  this.m_constraints = new Array();

  var i = 0;
  var tVec;
  var tMat;

  this.m_constraintCount = 0;
  for (i = 0; i < contactCount; ++i) {
    this.m_constraintCount += contacts[i].GetManifoldCount();
  }

  // fill array
  for (i = 0; i < this.m_constraintCount; i++) {
    this.m_constraints[i] = new box2d.ContactConstraint();
  }

  var count = 0;
  for (i = 0; i < contactCount; ++i) {
    var contact = contacts[i];
    var b1 = contact.m_shape1.m_body;
    var b2 = contact.m_shape2.m_body;
    var manifoldCount = contact.GetManifoldCount();
    var manifolds = contact.GetManifolds();
    var friction = contact.m_friction;
    var restitution = contact.m_restitution;

    //var v1 = b1.m_linearVelocity.Copy();
    var v1X = b1.m_linearVelocity.x;
    var v1Y = b1.m_linearVelocity.y;
    //var v2 = b2.m_linearVelocity.Copy();
    var v2X = b2.m_linearVelocity.x;
    var v2Y = b2.m_linearVelocity.y;
    var w1 = b1.m_angularVelocity;
    var w2 = b2.m_angularVelocity;

    for (var j = 0; j < manifoldCount; ++j) {
      var manifold = manifolds[j];

      //box2d.Settings.b2Assert(manifold.pointCount > 0);
      //var normal = manifold.normal.Copy();
      var normalX = manifold.normal.x;
      var normalY = manifold.normal.y;

      //box2d.Settings.b2Assert(count < this.m_constraintCount);
      var c = this.m_constraints[count];
      c.body1 = b1;
      c.body2 = b2;
      c.manifold = manifold;
      //c.normal = normal;
      c.normal.x = normalX;
      c.normal.y = normalY;
      c.pointCount = manifold.pointCount;
      c.friction = friction;
      c.restitution = restitution;

      for (var k = 0; k < c.pointCount; ++k) {
        var cp = manifold.points[k];
        var ccp = c.points[k];

        ccp.normalImpulse = cp.normalImpulse;
        ccp.tangentImpulse = cp.tangentImpulse;
        ccp.separation = cp.separation;

        //var r1 = box2d.Vec2.subtract( cp.position, b1.m_position );
        var r1X = cp.position.x - b1.m_position.x;
        var r1Y = cp.position.y - b1.m_position.y;
        //var r2 = box2d.Vec2.subtract( cp.position, b2.m_position );
        var r2X = cp.position.x - b2.m_position.x;
        var r2Y = cp.position.y - b2.m_position.y;

        //ccp.localAnchor1 = box2d.Math.b2MulTMV(b1.m_R, r1);
        tVec = ccp.localAnchor1;
        tMat = b1.m_R;
        tVec.x = r1X * tMat.col1.x + r1Y * tMat.col1.y;
        tVec.y = r1X * tMat.col2.x + r1Y * tMat.col2.y;

        //ccp.localAnchor2 = box2d.Math.b2MulTMV(b2.m_R, r2);
        tVec = ccp.localAnchor2;
        tMat = b2.m_R;
        tVec.x = r2X * tMat.col1.x + r2Y * tMat.col1.y;
        tVec.y = r2X * tMat.col2.x + r2Y * tMat.col2.y;

        var r1Sqr = r1X * r1X + r1Y * r1Y;
        var r2Sqr = r2X * r2X + r2Y * r2Y;

        //var rn1 = goog.math.Vec2.dot(r1, normal);
        var rn1 = r1X * normalX + r1Y * normalY;
        //var rn2 = goog.math.Vec2.dot(r2, normal);
        var rn2 = r2X * normalX + r2Y * normalY;
        var kNormal = b1.m_invMass + b2.m_invMass;
        kNormal += b1.m_invI * (r1Sqr - rn1 * rn1) + b2.m_invI * (r2Sqr - rn2 * rn2);
        //box2d.Settings.b2Assert(kNormal > Number.MIN_VALUE);
        ccp.normalMass = 1.0 / kNormal;

        //var tangent = box2d.Math.b2CrossVF(normal, 1.0);
        var tangentX = normalY;
        var tangentY = -normalX;

        //var rt1 = goog.math.Vec2.dot(r1, tangent);
        var rt1 = r1X * tangentX + r1Y * tangentY;
        //var rt2 = goog.math.Vec2.dot(r2, tangent);
        var rt2 = r2X * tangentX + r2Y * tangentY;
        var kTangent = b1.m_invMass + b2.m_invMass;
        kTangent += b1.m_invI * (r1Sqr - rt1 * rt1) + b2.m_invI * (r2Sqr - rt2 * rt2);
        //box2d.Settings.b2Assert(kTangent > Number.MIN_VALUE);
        ccp.tangentMass = 1.0 / kTangent;

        // Setup a velocity bias for restitution.
        ccp.velocityBias = 0.0;
        if (ccp.separation > 0.0) {
          ccp.velocityBias = -60.0 * ccp.separation;
        }
        //var vRel = goog.math.Vec2.dot(c.normal, box2d.Vec2.subtract( box2d.Vec2.subtract( box2d.Vec2.add( v2, box2d.Vec2.crossScalar(w2, r2)), v1 ), box2d.Vec2.crossScalar(w1, r1)));
        var tX = v2X + (-w2 * r2Y) - v1X - (-w1 * r1Y);
        var tY = v2Y + (w2 * r2X) - v1Y - (w1 * r1X);
        //var vRel = b2Dot(c.normal, tX/Y);
        var vRel = c.normal.x * tX + c.normal.y * tY;
        if (vRel < -box2d.Settings.b2_velocityThreshold) {
          ccp.velocityBias += -c.restitution * vRel;
        }
      }

      ++count;
    }
  }

  //box2d.Settings.b2Assert(count == this.m_constraintCount);
};

box2d.ContactSolver.prototype = {
  //~box2d.ContactSolver();
  PreSolve: function() {
    var tVec;
    var tVec2;
    var tMat;

    // Warm start.
    for (var i = 0; i < this.m_constraintCount; ++i) {
      var c = this.m_constraints[i];

      var b1 = c.body1;
      var b2 = c.body2;
      var invMass1 = b1.m_invMass;
      var invI1 = b1.m_invI;
      var invMass2 = b2.m_invMass;
      var invI2 = b2.m_invI;
      //var normal = new box2d.Vec2(c.normal.x, c.normal.y);
      var normalX = c.normal.x;
      var normalY = c.normal.y;
      //var tangent = box2d.Math.b2CrossVF(normal, 1.0);
      var tangentX = normalY;
      var tangentY = -normalX;

      var j = 0;
      var tCount = 0;
      if (box2d.World.s_enableWarmStarting) {
        tCount = c.pointCount;
        for (j = 0; j < tCount; ++j) {
          var ccp = c.points[j];
          //var P = box2d.Vec2.add( box2d.Vec2.multiplyScalar(ccp.normalImpulse, normal), box2d.Vec2.multiplyScalar(ccp.tangentImpulse, tangent));
          var PX = ccp.normalImpulse * normalX + ccp.tangentImpulse * tangentX;
          var PY = ccp.normalImpulse * normalY + ccp.tangentImpulse * tangentY;

          //var r1 = box2d.Math.b2MulMV(b1.m_R, ccp.localAnchor1);
          tMat = b1.m_R;
          tVec = ccp.localAnchor1;
          var r1X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
          var r1Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;

          //var r2 = box2d.Math.b2MulMV(b2.m_R, ccp.localAnchor2);
          tMat = b2.m_R;
          tVec = ccp.localAnchor2;
          var r2X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
          var r2Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;

          //b1.m_angularVelocity -= invI1 * box2d.Math.b2CrossVV(r1, P);
          b1.m_angularVelocity -= invI1 * (r1X * PY - r1Y * PX);
          //b1.m_linearVelocity.subtract( box2d.Vec2.multiplyScalar(invMass1, P) );
          b1.m_linearVelocity.x -= invMass1 * PX;
          b1.m_linearVelocity.y -= invMass1 * PY;
          //b2.m_angularVelocity += invI2 * box2d.Math.b2CrossVV(r2, P);
          b2.m_angularVelocity += invI2 * (r2X * PY - r2Y * PX);
          //b2.m_linearVelocity.Add( box2d.Vec2.multiplyScalar(invMass2, P) );
          b2.m_linearVelocity.x += invMass2 * PX;
          b2.m_linearVelocity.y += invMass2 * PY;

          ccp.positionImpulse = 0.0;
        }
      } else {
        tCount = c.pointCount;
        for (j = 0; j < tCount; ++j) {
          var ccp2 = c.points[j];
          ccp2.normalImpulse = 0.0;
          ccp2.tangentImpulse = 0.0;

          ccp2.positionImpulse = 0.0;
        }
      }
    }
  },
  SolveVelocityConstraints: function() {
    var j = 0;
    var ccp;
    var r1X;
    var r1Y;
    var r2X;
    var r2Y;
    var dvX;
    var dvY;
    var lambda;
    var newImpulse;
    var PX;
    var PY;

    var tMat;
    var tVec;

    for (var i = 0; i < this.m_constraintCount; ++i) {
      var c = this.m_constraints[i];
      var b1 = c.body1;
      var b2 = c.body2;
      var b1_angularVelocity = b1.m_angularVelocity;
      var b1_linearVelocity = b1.m_linearVelocity;
      var b2_angularVelocity = b2.m_angularVelocity;
      var b2_linearVelocity = b2.m_linearVelocity;

      var invMass1 = b1.m_invMass;
      var invI1 = b1.m_invI;
      var invMass2 = b2.m_invMass;
      var invI2 = b2.m_invI;
      //var normal = new box2d.Vec2(c.normal.x, c.normal.y);
      var normalX = c.normal.x;
      var normalY = c.normal.y;
      //var tangent = box2d.Math.b2CrossVF(normal, 1.0);
      var tangentX = normalY;
      var tangentY = -normalX;

      // Solver normal constraints
      var tCount = c.pointCount;
      for (j = 0; j < tCount; ++j) {
        ccp = c.points[j];

        //r1 = box2d.Math.b2MulMV(b1.m_R, ccp.localAnchor1);
        tMat = b1.m_R;
        tVec = ccp.localAnchor1;
        r1X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
        r1Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
        //r2 = box2d.Math.b2MulMV(b2.m_R, ccp.localAnchor2);
        tMat = b2.m_R;
        tVec = ccp.localAnchor2;
        r2X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
        r2Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;

        // Relative velocity at contact
        //var dv = box2d.Vec2.subtract( box2d.Vec2.add( b2.m_linearVelocity, box2d.Vec2.crossScalar(b2.m_angularVelocity, r2)), box2d.Vec2.subtract(b1.m_linearVelocity, box2d.Vec2.crossScalar(b1.m_angularVelocity, r1)));
        //dv = box2d.Vec2.subtract(box2d.Vec2.subtract( box2d.Vec2.add( b2.m_linearVelocity, box2d.Vec2.crossScalar(b2.m_angularVelocity, r2)), b1.m_linearVelocity), box2d.Vec2.crossScalar(b1.m_angularVelocity, r1));
        dvX = b2_linearVelocity.x + (-b2_angularVelocity * r2Y) - b1_linearVelocity.x - (-b1_angularVelocity * r1Y);
        dvY = b2_linearVelocity.y + (b2_angularVelocity * r2X) - b1_linearVelocity.y - (b1_angularVelocity * r1X);

        // Compute normal impulse
        //var vn = goog.math.Vec2.dot(dv, normal);
        var vn = dvX * normalX + dvY * normalY;
        lambda = -ccp.normalMass * (vn - ccp.velocityBias);

        // b2Clamp the accumulated impulse
        newImpulse = Math.max(ccp.normalImpulse + lambda, 0.0);
        lambda = newImpulse - ccp.normalImpulse;

        // Apply contact impulse
        //P = box2d.Vec2.multiplyScalar(lambda, normal);
        PX = lambda * normalX;
        PY = lambda * normalY;

        //b1.m_linearVelocity.subtract( box2d.Vec2.multiplyScalar( invMass1, P ) );
        b1_linearVelocity.x -= invMass1 * PX;
        b1_linearVelocity.y -= invMass1 * PY;
        b1_angularVelocity -= invI1 * (r1X * PY - r1Y * PX);

        //b2.m_linearVelocity.Add( box2d.Vec2.multiplyScalar( invMass2, P ) );
        b2_linearVelocity.x += invMass2 * PX;
        b2_linearVelocity.y += invMass2 * PY;
        b2_angularVelocity += invI2 * (r2X * PY - r2Y * PX);

        ccp.normalImpulse = newImpulse;

        // MOVED FROM BELOW
        // Relative velocity at contact
        //var dv = b2.m_linearVelocity + b2Cross(b2.m_angularVelocity, r2) - b1.m_linearVelocity - b2Cross(b1.m_angularVelocity, r1);
        //dv =  box2d.Vec2.subtract(box2d.Vec2.subtract(box2d.Vec2.add(b2.m_linearVelocity, box2d.Vec2.crossScalar(b2.m_angularVelocity, r2)), b1.m_linearVelocity), box2d.Vec2.crossScalar(b1.m_angularVelocity, r1));
        dvX = b2_linearVelocity.x + (-b2_angularVelocity * r2Y) - b1_linearVelocity.x - (-b1_angularVelocity * r1Y);
        dvY = b2_linearVelocity.y + (b2_angularVelocity * r2X) - b1_linearVelocity.y - (b1_angularVelocity * r1X);

        // Compute tangent impulse
        var vt = dvX * tangentX + dvY * tangentY;
        lambda = ccp.tangentMass * (-vt);

        // b2Clamp the accumulated impulse
        var maxFriction = c.friction * ccp.normalImpulse;
        newImpulse = box2d.Math.b2Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction);
        lambda = newImpulse - ccp.tangentImpulse;

        // Apply contact impulse
        //P = box2d.Vec2.multiplyScalar(lambda, tangent);
        PX = lambda * tangentX;
        PY = lambda * tangentY;

        //b1.m_linearVelocity.subtract( box2d.Vec2.multiplyScalar( invMass1, P ) );
        b1_linearVelocity.x -= invMass1 * PX;
        b1_linearVelocity.y -= invMass1 * PY;
        b1_angularVelocity -= invI1 * (r1X * PY - r1Y * PX);

        //b2.m_linearVelocity.Add( box2d.Vec2.multiplyScalar( invMass2, P ) );
        b2_linearVelocity.x += invMass2 * PX;
        b2_linearVelocity.y += invMass2 * PY;
        b2_angularVelocity += invI2 * (r2X * PY - r2Y * PX);

        ccp.tangentImpulse = newImpulse;
      }

      // Solver tangent constraints
      // MOVED ABOVE FOR EFFICIENCY
      /*for (j = 0; j < tCount; ++j)
      {
        ccp = c.points[ j ];

        //r1 = box2d.Math.b2MulMV(b1.m_R, ccp.localAnchor1);
        tMat = b1.m_R;
        tVec = ccp.localAnchor1;
        r1X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y
        r1Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y
        //r2 = box2d.Math.b2MulMV(b2.m_R, ccp.localAnchor2);
        tMat = b2.m_R;
        tVec = ccp.localAnchor2;
        r2X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y
        r2Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y

        // Relative velocity at contact
        //var dv = b2.m_linearVelocity + b2Cross(b2.m_angularVelocity, r2) - b1.m_linearVelocity - b2Cross(b1.m_angularVelocity, r1);
        //dv =  box2d.Vec2.subtract(box2d.Vec2.subtract(box2d.Vec2.add(b2.m_linearVelocity, box2d.Vec2.crossScalar(b2.m_angularVelocity, r2)), b1.m_linearVelocity), box2d.Vec2.crossScalar(b1.m_angularVelocity, r1));
        dvX = b2_linearVelocity.x + (-b2_angularVelocity * r2Y) - b1_linearVelocity.x - (-b1_angularVelocity * r1Y);
        dvY = b2_linearVelocity.y + (b2_angularVelocity * r2X) - b1_linearVelocity.y - (b1_angularVelocity * r1X);

        // Compute tangent impulse
        var vt = dvX*tangentX + dvY*tangentY;
        lambda = ccp.tangentMass * (-vt);

        // b2Clamp the accumulated impulse
        var maxFriction = c.friction * ccp.normalImpulse;
        newImpulse = box2d.Math.b2Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction);
        lambda = newImpulse - ccp.tangentImpulse;

        // Apply contact impulse
        //P = box2d.Vec2.multiplyScalar(lambda, tangent);
        PX = lambda * tangentX;
        PY = lambda * tangentY;

        //b1.m_linearVelocity.subtract( box2d.Vec2.multiplyScalar( invMass1, P ) );
        b1_linearVelocity.x -= invMass1 * PX;
        b1_linearVelocity.y -= invMass1 * PY;
        b1_angularVelocity -= invI1 * (r1X * PY - r1Y * PX);

        //b2.m_linearVelocity.Add( box2d.Vec2.multiplyScalar( invMass2, P ) );
        b2_linearVelocity.x += invMass2 * PX;
        b2_linearVelocity.y += invMass2 * PY;
        b2_angularVelocity += invI2 * (r2X * PY - r2Y * PX);

        ccp.tangentImpulse = newImpulse;
      }*/

      // Update angular velocity
      b1.m_angularVelocity = b1_angularVelocity;
      b2.m_angularVelocity = b2_angularVelocity;
    }
  },
  SolvePositionConstraints: function(beta) {
    var minSeparation = 0.0;

    var tMat;
    var tVec;

    for (var i = 0; i < this.m_constraintCount; ++i) {
      var c = this.m_constraints[i];
      var b1 = c.body1;
      var b2 = c.body2;
      var b1_position = b1.m_position;
      var b1_rotation = b1.m_rotation;
      var b2_position = b2.m_position;
      var b2_rotation = b2.m_rotation;

      var invMass1 = b1.m_invMass;
      var invI1 = b1.m_invI;
      var invMass2 = b2.m_invMass;
      var invI2 = b2.m_invI;
      //var normal = new box2d.Vec2(c.normal.x, c.normal.y);
      var normalX = c.normal.x;
      var normalY = c.normal.y;
      //var tangent = box2d.Math.b2CrossVF(normal, 1.0);
      var tangentX = normalY;
      var tangentY = -normalX;

      // Solver normal constraints
      var tCount = c.pointCount;
      for (var j = 0; j < tCount; ++j) {
        var ccp = c.points[j];

        //r1 = box2d.Math.b2MulMV(b1.m_R, ccp.localAnchor1);
        tMat = b1.m_R;
        tVec = ccp.localAnchor1;
        var r1X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
        var r1Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
        //r2 = box2d.Math.b2MulMV(b2.m_R, ccp.localAnchor2);
        tMat = b2.m_R;
        tVec = ccp.localAnchor2;
        var r2X = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
        var r2Y = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;

        //var p1 = box2d.Vec2.add(b1.m_position, r1);
        var p1X = b1_position.x + r1X;
        var p1Y = b1_position.y + r1Y;

        //var p2 = box2d.Vec2.add(b2.m_position, r2);
        var p2X = b2_position.x + r2X;
        var p2Y = b2_position.y + r2Y;

        //var dp = box2d.Vec2.subtract(p2, p1);
        var dpX = p2X - p1X;
        var dpY = p2Y - p1Y;

        // Approximate the current separation.
        //var separation = goog.math.Vec2.dot(dp, normal) + ccp.separation;
        var separation = (dpX * normalX + dpY * normalY) + ccp.separation;

        // Track max constraint error.
        minSeparation = Math.min(minSeparation, separation);

        // Prevent large corrections and allow slop.
        var C = beta * box2d.Math.b2Clamp(separation + box2d.Settings.b2_linearSlop, -box2d.Settings.b2_maxLinearCorrection, 0.0);

        // Compute normal impulse
        var dImpulse = -ccp.normalMass * C;

        // b2Clamp the accumulated impulse
        var impulse0 = ccp.positionImpulse;
        ccp.positionImpulse = Math.max(impulse0 + dImpulse, 0.0);
        dImpulse = ccp.positionImpulse - impulse0;

        //var impulse = box2d.Vec2.multiplyScalar( dImpulse, normal );
        var impulseX = dImpulse * normalX;
        var impulseY = dImpulse * normalY;

        //b1.m_position.subtract( box2d.Vec2.multiplyScalar( invMass1, impulse ) );
        b1_position.x -= invMass1 * impulseX;
        b1_position.y -= invMass1 * impulseY;
        b1_rotation -= invI1 * (r1X * impulseY - r1Y * impulseX);
        b1.m_R.Set(b1_rotation);

        //b2.m_position.Add( box2d.Vec2.multiplyScalar( invMass2, impulse ) );
        b2_position.x += invMass2 * impulseX;
        b2_position.y += invMass2 * impulseY;
        b2_rotation += invI2 * (r2X * impulseY - r2Y * impulseX);
        b2.m_R.Set(b2_rotation);
      }
      // Update body rotations
      b1.m_rotation = b1_rotation;
      b2.m_rotation = b2_rotation;
    }

    return minSeparation >= -box2d.Settings.b2_linearSlop;
  },
  PostSolve: function() {
    for (var i = 0; i < this.m_constraintCount; ++i) {
      var c = this.m_constraints[i];
      var m = c.manifold;

      for (var j = 0; j < c.pointCount; ++j) {
        var mPoint = m.points[j];
        var cPoint = c.points[j];
        mPoint.normalImpulse = cPoint.normalImpulse;
        mPoint.tangentImpulse = cPoint.tangentImpulse;
      }
    }
  }
};
